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ABSTRACT 

This  paper  outlines  an  architecture  for  underwater  acoustic 
cooperative  localization.  Our  system  leverages  communica¬ 
tion  within  an  acoustic  network  to  both  share  navigation 
information  and  measure  the  relative  range  between  vehicles. 
We  employ  a  factor  graph  framework  for  vehicle  position 
estimation.  The  underlying  structure  of  the  factor  graph 
formulation  provides  a  low-bandwidth  estimation  framework 
that  is  tolerant  to  communication  dropout.  We  detail  the 
hardware  and  software  used  to  implement  a  three  vehicle 
cooperative  network  and  provide  a  performance  summary 
over  several  field  trials. 

Categories  and  Subject  Descriptors 

1.2.9  [Robotics]:  Autonomous  vehicles 

1.  INTRODUCTION 

Underwater  acoustic  communication  enables  subsea  vehi¬ 
cles  to  broadcast  and  receive  command,  control,  and  health 
information  from  surface  operators.  Additionally,  time-of- 
flight  (TOF)  measurements  of  acoustic  broadcasts  between 
vehicles  can  be  used  to  augment  an  underwater  navigation 
framework.  In  this  paper,  we  outline  the  design,  implemen¬ 
tation,  and  deployment  of  a  system  architecture  for  multiple 
vehicle  operations  that  exploits  acoustic  communication  for 
online  navigation. 

Underwater  vehicles  are  unable  to  directly  observe  XY 
position;  seawater  is  opaque  to  electromagnetic  signals  such 
that  standard  terrestrial  systems  like  the  global  positioning 
system  (GPS)  are  unavailable.  Underwater  vehicles  integrate 
body-frame  Doppler  velocity,  attitude,  and  depth  to  compute 
a  dead-reckoned  navigation  estimate.  XY  position  errors 
from  dead-reckoned  navigation,  however,  grow  unbounded 
in  time.  Bounded-error  navigation  can  be  achieved  using 

*This  work  was  supported  in  part  by  the  Office  of  Naval 
Research  under  award  N00014-12- 1-0092,  and  in  part  by  the 
National  Science  Foundation  under  grant  IIS-0746455. 

Permission  to  make  digital  or  hard  copies  of  all  or  part  of  this  work  for 
personal  or  classroom  use  is  granted  without  fee  provided  that  copies  are  not 
made  or  distributed  for  profit  or  commercial  advantage  and  that  copies  bear 
this  notice  and  the  full  citation  on  the  first  page.  Copyrights  for  components 
of  this  work  owned  by  others  than  ACM  must  be  honored.  Abstracting  with 
credit  is  permitted.  To  copy  otherwise,  or  republish,  to  post  on  servers  or  to 
redistribute  to  lists,  requires  prior  specific  permission  and/or  a  fee.  Request 
permissions  from  Permissions@acm.org. 

WUWNET  T5,  October  22-24  2015,  Washington  DC,  USA 
Copyright  2015  ACM  978-1-4503-4036-6/15/10  ...$15.00. 


vehicle  1 

N 

vehicle  M 

\ 

|single  vehicle 

arch| 

|single  vehicle 

arch| 

I 

t 

lAcomms  manager 

lAcomms  manager 

V 

V 

J 

$ 

1 

( 

Acoustic  channel 

) 

(a)  Multiple  vehicles  exchange  information  over  the 
acoustic  communication  channel  and  observe  their 
TOF-based  relative  range. 
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(b)  Independent  navigation  and  control  system  onboard 
Iver  AUVs.  The  cooperative  localization  process  is 
highlighted  in  red. 


Figure  1:  Block  diagram  of  a  multi- vehicle  system  architec¬ 
ture.  Single  vehicle  inter-process  communication  is  handled 
via  LCM.  Communication  between  multiple  vehicles  is  han¬ 
dled  over  the  acoustic  channel. 


static  acoustic  beacon  networks  such  as  narrowband  long- 
baseline  (LBL)  [1],  As  vehicle  network  size  increases,  however, 
the  rate  at  which  each  vehicle  can  query  the  LBL  network 
decreases.  Moreover,  the  range  of  vehicle  operations  is  limited 
by  the  range  of  the  LBL  network. 

With  the  addition  of  synchronous  clock  hardware,  vehicles 
in  an  acoustic  network  can  passively  measure  the  one-way- 
travel-time  (OWTT)  of  broadcast  messages.  Assuming  a 
known  sound  velocity  profile,  receiving  vehicles  observe  the 
relative  range  at  their  time-of-arrival  (TOA)  to  the  time-of- 
launch  (TOL)  position  of  the  broadcasting  vehicle.  Unlike 
static  beacon  networks,  OWTT-based  underwater  localiza¬ 
tion  requires  vehicles  to  share  state  information  across  the 
network.  Solutions  detailing  information  exchange  and  the 
resulting  estimation  frameworks  have  appeared  throughout 
the  literature  including  [2-8]. 

Below,  we  outline  a  recent  distributed  estimation  frame¬ 
work  for  multi- vehicle  OWTT  cooperative  localization  [8]. 
The  primary  contribution  of  this  paper  includes  the  details 


Figure  2:  Example  factor  graph  estimation  framework  and 
corresponding  measurement  Jacobian,  A  (left).  Each  row  of 
pose  nodes  (large  circles)  represents  a  single  vehicle  (right). 
Small  circles  represent  factors  (observations).  The  full  (cen¬ 
tralized)  graph  is  highlighted  in  gray,  while  the  reconstruction 
on  board  the  third  (purple)  vehicle  is  fully  colored. 

of  the  hardware  and  software  processes  employed  within  our 
system  and  their  integration  within  a  multi- vehicle  architec¬ 
ture  (Fig.  1).  We  also  provide  the  cooperative  localization 
functionality  as  an  open-source  software  library.  While  the 
architecture  described  is  applicable  to  a  wide  range  of  acous¬ 
tic  networks,  we  summarize  an  implementation  on  a  three 
vehicle  network.  Finally,  we  present  a  performance  summary 
over  several  held  trials. 

2.  COOPERATIVE  LOCALIZATION 

Cooperative  localization  refers  to  the  estimation  of  a  vehi¬ 
cle  state  given  relative  position  observations  to  other  vehicles. 
Vehicles  fuse  measurements  from  onboard  sensing  (e.g.,  veloc¬ 
ity  and  attitude)  with  external  information  such  as  relative 
vehicle  observations  and  shared  data.  Herein  relative  position 
observations  specifically  refer  to  OWTT  ranges. 

The  limitations  of  the  acoustic  communication  channel 
[9]  challenge  the  design  of  a  cooperative  localization  system. 
Chiefly,  low-bandwidth  and  unacknowledged  data  packets 
constrain  each  vehicle’s  ability  to  share  information  across 
the  network.  We  show  below  that  jointly  estimating  the  full 
trajectory  of  each  vehicle  leads  to  a  solution  that  can  be 
efficiently  distributed  across  an  acoustic  network. 

2.1  Factor  graph  trajectory  estimation 

We  apply  a  maximum-likelihood  approach  to  jointly  es¬ 
timate  the  trajectory  of  each  vehicle,  that  is,  we  compute 
the  set  of  vehicle  trajectories  that  best  explains  the  set  of 
observations.  We  use  a  factor  graph  to  represent  the  joint 
probability  distribution  over  vehicle  trajectories.  A  factor 
graph  is  a  bipartite  graphical  model  composed  of  variable 
nodes  (poses  along  a  trajectory)  and  factor  nodes  (measure¬ 
ments).  Factor  graphs  have  become  a  standard  within  the 
robotics  community  for  estimation,  in  particular,  for  the  si¬ 
multaneous  localization  and  mapping  (SLAM)  problem  [10]. 
For  a  more  thorough  derivation  of  the  below  framework,  the 
interested  reader  is  referred  to  [8]. 

Factor  graphs  represent  a  smoothing  approach  as  opposed 
to  standard  state  estimation,  which  only  estimates  the  current 
vehicle  state.  The  complete  trajectory  of  the  ith  vehicle  is 
represented  by  a  set  of  rigid-body  poses,  X,  =  {xi, . . . ,  Xjv}. 
To  minimize  bandwidth  in  our  distributed  system,  each  rigid- 
body  pose  is  represented  as  its  XY  position.  The  distribution 
over  all  poses  can  be  factored  given  the  set  of  all  local  obser¬ 
vations,  Z  i,  as 

p(Xj|Zj)  0Cp(xi)]^[p(zodoi|xi_l,Xi)]^[p(Zpriorj.  |Xj).  (1) 

i  3 


We  only  consider  two  factor  types:  unary  ‘prior’  factors  (such 
as  GPS)  and  binary  ‘odometry’  factors  (integrated  velocity). 
The  structure  of  the  single  vehicle  factor  graph  is  a  chain. 
Herein,  the  ith  vehicle’s  local  factor  graph  will  be  referred  to 
as  factor  chain  Ci .  Poses  along  the  chain  with  their  associated 
factors  are  referred  to  as  links.  In  Fig.  2,  each  row  of  pose 
nodes  represents  a  separate  vehicle  chain. 

When  considering  the  factor  graph  over  the  full  vehicle 
network,  OWTT  relative  range  measurements  introduce  con¬ 
straints  between  the  broadcasting  vehicle  TOL  pose  and  the 
receiving  vehicle  TOA  pose.  Within  the  factor  graph  formu¬ 
lation,  the  joint  distribution  over  the  set  of  poses  for  each  of 
M  vehicles  can  be  factored  over  the  set  of  vehicle  chains  and 
relative  ranges 
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where  Zr  is  the  set  of  relative  range  factors.  Fig.  2  illustrates 
the  factor  graph  over  a  three  vehicle  network. 

Each  vehicle  estimates  its  trajectory  given  the  full  set  of 
factors.  For  zero-mean  additive  Gaussian  noise  models,  the 
maximum  likelihood  estimate  results  in  a  nonlinear  least- 
squares  problem  with  linear  subproblem 

min  ||  AX  —  b  1 1 2  ,  (3) 

where  A  is  the  sparse  measurement  Jacobian  weighted  by 
the  square  root  information  [10].  A  is  the  matrix  analogue 
of  the  factor  graph  (Fig.  2).  Sparsity  here  implies  efficient 
solutions. 

We  can  exploit  the  factorization  of  the  joint  distribution 
over  all  vehicle  poses,  to  arrive  at  a  distributed  solution. 
Within  our  estimation  framework,  each  vehicle  shares  por¬ 
tions  of  its  local  chain  with  the  network1.  Properties  of 
the  constituent  factors  allow  for  low-bandwidth  and  dropout 
tolerant  communication.  In  turn,  each  vehicle  can  then 
reconstruct  a  portion  of  the  global  factor  graph  using  its 
local  chain,  the  set  of  received  chains,  and  the  set  of  locally 
observed  relative  range  factors  (Fig.  2). 

3.  SYSTEM  ARCHITECTURE 

Our  multiple  vehicle  system  architecture  spans  two  sub¬ 
systems  (Fig.  1):  a  single  vehicle  module  containing  sensor 
drivers,  navigation,  and  control  processes  and  a  multiple 
vehicle  module  encompassing  interaction  between  vehicles 
over  the  acoustic  channel. 

We  discuss  the  system  architecture  as  implemented  on  a 
three  vehicle  network  consisting  of  two  Ocean  Server,  Inc. 
Iver2  AUVs  (Fig.  3)  and  a  topside  ship.  While  we  make  spe¬ 
cific  notes  about  this  three  vehicle  network,  the  architecture 
is  vehicle  independent. 

3.1  Single  vehicle  subsystem 

Each  vehicle  executes  several  processes  including  sensor 
drivers,  a  pose  estimator  (Section  2),  and,  in  the  case  of  the 
AUVs,  a  vehicle  controller.  The  vehicle  software  architecture 
is  built  around  several  open  source  projects,  most  notably 

xThe  described  approach  only  shares  local  vehicle  informa¬ 
tion.  Avenues  for  future  work  include  forwarding  information 
from  the  wider  vehicle  network. 


Figure  3:  Iver2  AUVs  used  within  the  described  cooperative 
acoustic  network. 


lightweight  communication  and  marshalling  (LCM)  [11]  for 
inter-process  communication.  The  single  vehicle  architecture 
is  outlined  in  Fig.  lb  and  described  below. 

We  rely  on  LCM  to  promote  a  modular  software  architec¬ 
ture  in  which  sensor  drivers,  estimators,  and  controllers  may 
be  designed  as  separate  processes.  LCM  allows  processes  to 
communicate  over  user  datagram  protocol  (UDP)  multicast 
within  a  publish/subscribe  paradigm.  Processes  can  publish 
user-defined  message  types  to  subscribers.  LCM  provides 
additional  benefits  including  logging  and  playback  utilities, 
a  real-time  messaging  spy,  and  wrappers  for  several  popular 
languages  including  C/C-l — h,  Java,  and  Python.  Logging 
and  playback  are  particularly  well  suited  for  robotics  to  aid 
with  development  and  post  processing. 

We  field  two  modified  Iver2  AUVs  detailed  in  [12].  Each 
AUV  is  driven  by  the  Ocean  Server  controller,  Underwater 
Vehicle  Console  (UVC).  UVC  provides  an  interface  to  a 
‘backseat’  CPU  [13]  over  a  serial  communication  link.  Our 
backseat  Linux  host  is  responsible  for  all  sensor  processing 
and  acoustic  communication.  Additionally,  the  backseat 
executes  a  state  machine  and  health  monitor  to  automate 
data  logging  and  handle  vehicle  behavior.  All  communi¬ 
cation  to  the  UVC  is  piped  through  the  Backseat  driver 
process  (Fig.  lb),  which  translates  between  UVC  and  the 
LCM  network. 

Our  topside  vehicle  executes  sensor  and  acoustic  communi¬ 
cation  software  on  a  Linux  host.  The  software  stack  onboard 
the  topside  vehicle  is  nearly  identical  to  each  AUV  with  the 
exception  of  the  vehicle  controller.  We  also  run  a  viewer 
and  acoustic  communication  user  interface  to  visualize  AUV 
state  information  provided  by  periodic  acoustic  broadcasts 
and  submit  control  commands. 

3.2  Multiple  vehicle  subsystem 

The  multiple  vehicle  subsystem  includes  all  hardware  and 
software  processes  that  interface  directly  with  the  acous¬ 
tic  communication  system.  While  LCM  handles  communi¬ 
cation  between  processes  on  a  single  vehicle,  the  acoustic 
hardware  and  software  handle  communication  between  vehi¬ 
cles.  Multiple  vehicles  interact  via  periodic  acoustic  broad¬ 
casts.  Our  architecture  is  similar  to  the  one  described  by 
[14].  Each  vehicle  is  equipped  with  a  Woods  Hole  Oceano¬ 
graphic  Institution  (WHOI)  Micro-modem  and  co-processor 
board  [15],  25  kHz  BTech  Acoustics  2RCL  transducer,  and  a 
synchronous-clock  reference.  The  Micro-modem  is  configured 
to  operate  in  a  synchronous  clock  mode,  using  the  user  sup¬ 


plied  pulse  per  second  (PPS)  signal  to  discipline  its  internal 
clock. 

Stable  synchronized  clock  references  serve  as  the  corner¬ 
stone  for  the  acoustic  network  onboard  each  vehicle.  The 
topside  ship  clock  is  disciplined  by  a  Meinberg  GPS  time¬ 
server.  Each  Iver2  AUV  is  outfitted  with  a  PPSBoard  [16], 
which  provides  a  free-running  low-drift  PPS  signal  (aligned 
to  a  GPS  time  signal  while  at  the  surface)  while  the  vehicles 
are  subsea.  The  PPSBoard  is  built  around  a  SeaScan,  Inc.., 
temperature  compensated  crystal  oscillator  which  produces 
less  than  1  ms  drift  over  14  h  (corresponding  to  a  —  1.5  m 
bias  in  range  observations).  Newer  commercially  available 
free-running  clocks  provide  improved  performance.  For  exam¬ 
ple,  Symmetricom  provides  less  than  1  ms  drift  over  5000  li 
(-208  d). 

Our  acoustic  communication  software,  Acomms  manager  in 
Fig.  la,  is  built  around  the  C++  Goby  acomms  library  [17] 
for  medium  access  (MAC)  scheduling  and  data  quantization 
and  serialization.  During  a  mission,  each  vehicle  executes  a 
preconfigured  fixed  time  division  multiple  access  (TDMA) 
schedule.  Each  vehicle  is  assigned  a  slot,  or  several  slots, 
within  the  TDMA  period  to  broadcast  messages.  Slots  are 
preconfigured  for  specific  message  types,  for  example,  LBL 
pings  or  varying  size  data  packets.  We  frequently  use  a  simple 
TDMA  in  which  vehicles  broadcast  a  state  data  packet  and  a 
navigation  data  packet  roughly  once  per  minute.  The  Goby 
MAC  library  is  responsible  for  signaling  transmissions  time 
so  that  messages  are  broadcast  at  the  top  of  the  second  for 
synchronous  communication. 

The  Micro-modem  with  the  co-processor  board  is  capable 
of  broadcasting  various  length  frequency-shift  keying  (FSK) 
or  phase-shift  keying  (PSK)  data  packets[15].  We  typically 
use  FSK  rate  0  Micro-modem  (one  32  Byte  data  frame)  data 
packets  to  broadcast  vehicle  state  and  health  to  the  topside 
ship.  The  topside  ship  uses  a  variety  of  13  bit  mini  and 
32  Byte  data  packets  for  control.  We  use  rate  1  and  rate  2 
PSK  data  packets  (three  64  Byte  data  frames)  to  relay  navi¬ 
gation  links  for  factor  graph-based  cooperative  localization. 

The  Goby  dynamic  compact  control  language  (DCCL) 
library  [18]  handles  quantizing  and  marshalling  data  messages 
for  acoustic  communication.  Goby  relies  on  Google  protobufs 
to  specify  the  range  and  resolution  of  each  field  within  a 
specific  message  type.  Our  acoustic  communication  process 
translates  between  LCM  message  types  on  the  local  vehicle 
and  a  DCCL  type  for  broadcast.  We  employ  message  types 
for  navigation  data,  state  (e.g.,  estimated  position,  depth, 
battery  health),  and  command  (e.g.,  abort,  abort-to-surface, 
jump  to  waypoint,  manual  override). 

3.2.1  Estimator  implementation 

Our  implementation  of  the  distributed  factor  graph  ap¬ 
proach  (Section  2)  is  built  around  the  incremental  smooth¬ 
ing  and  mapping  (iSAM)  [10]  library  and  is  released  as  an 
open-source  C++  library2.  iSAM  updates  the  weighted  mea¬ 
surement  Jacobian  to  efficiently  recover  a  navigation  solution. 
The  factor  graph  approach  can  easily  include  additional  fac¬ 
tor  types  in  each  vehicle’s  local  graph.  For  example,  we 
can  incorporate  ranges  to  LBL  beacons  by  including  LBL 
nodes  along  with  the  estimated  vehicle  trajectories.  Running 
on  relatively  modest  hardware,  an  Intel  Core  2  Duo  CPU, 
the  Estimator  process  required  approximately  3%  CPU  load 
during  our  field  trials. 

2http:  / /robots. engin.umich.edu/SoftwareData/OWTT 


(a)  Relative  vehicle  trajectories  (topside  not  shown). 


(b)  AUV-B’s  estimate  uncertainty. 


Figure  4:  Summary  of  field  trial  and  performance  comparison, 
(a)  An  XY  view  of  the  vehicle  trajectories.  Blue  dots  indicate 
where  AUV-B  received  range  observations,  (b)  The  smoothed 
uncertainty  in  each  AUV-B  pose  as  the  fourth  root  of  the 
determinant  of  the  pose  marginal  covariance. 


The  cooperative  localization  process,  Estimator  in  Fig.  lb, 
interfaces  with  the  acoustic  communication  Acomms  manager 
to  determine  when  to  add  pose  nodes  to  the  graph  and  to 
share  navigation  links  over  the  network.  The  Estimator 
packs  odometry  and  GPS  prior  factors  by  subscribing  to  the 
sensor  driver  LCM  stream. 

At  the  TOL,  the  Acomms  manager  requests  a  data  packet 
from  the  Estimator.  The  Estimator  adds  a  TOL  pose  node 
to  the  graph  and  computes  the  navigation  information  to  be 
broadcast.  At  the  TOA  of  a  data  packet,  the  Acomms  man¬ 
ager  publishes  the  decoded  navigation  data  and  the  OWTT 
range  observation  to  the  Estimator,  which,  in  turn,  adds  the 
new  factors  and  pose  nodes  to  its  graph.  Following  OWTT 
measurement  updates,  the  Estimator  publishes  the  current 
vehicle  state  estimate  to  the  Backseat  driver  so  that  UVC 
can  correct  for  navigation  error.  A  similar  interaction  occurs 
when  an  LBL  range  observation  is  obtained.  We  leverage 
robust  cost  functions,  specifically  dynamic  covariance  scaling 
[19],  within  the  factor  graph  to  reduce  the  influence  of  outlier 
range  observations. 


Figure  5:  AUV  trajectory  (1  h)  with  topside  OWTT  and 
LBL  support.  Blue  dots  indicate  where  range  observations 
were  received.  The  inset  plots  LBL  beacon  locations. 

4.  FIELD  TRIALS 

The  described  multi-vehicle  architecture  has  been  success¬ 
fully  deployed  in  several  shallow  water  field  trials  carried  out 
at  the  University  of  Michigan  Biological  Station.  We  summa¬ 
rize  two  field  trials  below  highlighting  typical  configurations 
for  multiple  vehicle  operations.  For  convenience,  we  refer  to 
each  Iver2  AUV  as  AUV-A  and  AUV-B. 

4.1  Two  AUV  deployment 

The  first  trial  represents  a  typical  three  vehicle  deployment. 
The  topside  vehicle  supported  AUV-A  and  AUV-B  during 
a  1.5  h  mission.  AUV-A  followed  a  large  diamond  at  a 
fixed  depth  of  8  m  over  AUV-B’s  lawnmower  survey  (fixed 
depth  6  m)  while  the  topside  vehicle  drifted  above  the  survey 
area  (Fig.  4a).  AUV-A  had  periodic  access  to  GPS  during 
brief  surface  intervals.  The  factor  graph  produced  here  is 
illustrated  in  Fig.  2.  All  vehicles  remained  within  500  m 
during  the  trial.  Acoustic  reception  rates  were  asymmetric 
between  the  vehicles,  ranging  between  37-87%. 

Fig.  4b  shows  the  resulting  uncertainty  over  trajectory 
poses  for  dead-reckoning,  the  post-process  centralized  estima¬ 
tor,  and  the  distributed  factor  graph  framework.  Although 
AUV-B  used  only  the  local  subset  of  range  factors,  its  esti¬ 
mate  was  able  to  benefit  from  relative  range  observations 
and  the  difference  compared  to  the  centralized  estimator  is 
small. 

4.2  Single  AUV  with  LBL  support 

The  second  trial  highlights  the  ability  of  our  navigation 
architecture  to  include  additional  navigation  constraints.  In 
this  trial,  a  single  AUV,  AUV-A,  executed  a  lawn-mower 
survey  at  a  constant  5  m  depth.  The  topside  ship  provided 
OWTT  support.  AUV-A  also  interrogated  the  three-beacon 
LBL  network  roughly  once  per  minute.  44%  of  acoustic 
broadcasts  were  successfully  received  between  the  vehicles. 
The  estimated  AUV  position  fuses  only  acoustic  ranges  and 
dead-reckoned  navigation.  During  the  trial,  the  AUV  peri¬ 
odically  received  GPS  for  post-process  comparison. 


The  estimated  trajectory  is  shown  in  Fig.  5  along  with 
the  dead-reckoned  only  navigation  result.  We  post-processed 
optimized  the  AUV  trajectory  with  GPS  observations  to 
compare  to  the  accuracy  of  the  acoustic  navigation  solution. 
The  on-line  solution  using  acoustic  ranges  is  able  to  closely 
follow  the  GPS  optimized  result  and  shows  clear  improvement 
of  the  dead-reckoned  solution,  which  drifts  approximately 
10  m  by  the  end  of  the  mission. 

5.  CONCLUSIONS 

As  AUV  network  sizes  grow,  cooperative  architectures  will 
serve  an  important  role  in  monitoring  underwater  vehicles. 
The  cooperative  localization  system  architecture  described 
is  extensible  and  vehicle  independent.  We  have  successfully 
deployed  this  system  during  several  field  trials  with  a  three 
vehicle  network  and  demonstrated  the  ability  to  augment 
dead-reckoned  navigation  with  OWTT  range  constraints  be¬ 
tween  vehicles.  Moreover,  the  acoustic  communication  sys¬ 
tem  allows  topside  operators  to  monitor  AUV  state  and 
health  as  well  as  broadcast  commands. 
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